<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<title>OpenShoe: ZUPT aided INS functions</title>

<link href="tabs.css" rel="stylesheet" type="text/css"/>
<link href="doxygen.css" rel="stylesheet" type="text/css" />

<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="search/search.js"></script>
<script type="text/javascript">
  $(document).ready(function() { searchBox.OnSelectItem(0); });
</script>

</head>
<body>
<div id="top"><!-- do not remove this div! -->


<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  
  
  <td style="padding-left: 0.5em;">
   <div id="projectname">OpenShoe
   &#160;<span id="projectnumber">1.0</span>
   </div>
   
  </td>
  
  
  
 </tr>
 </tbody>
</table>
</div>

<!-- Generated by Doxygen 1.7.5.1 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "search",false,'Search');
</script>
  <div id="navrow1" class="tabs">
    <ul class="tablist">
      <li><a href="index.html"><span>Main&#160;Page</span></a></li>
      <li><a href="pages.html"><span>Related&#160;Pages</span></a></li>
      <li><a href="modules.html"><span>Modules</span></a></li>
      <li><a href="annotated.html"><span>Data&#160;Structures</span></a></li>
      <li><a href="files.html"><span>Files</span></a></li>
      <li>
        <div id="MSearchBox" class="MSearchBoxInactive">
        <span class="left">
          <img id="MSearchSelect" src="search/mag_sel.png"
               onmouseover="return searchBox.OnSearchSelectShow()"
               onmouseout="return searchBox.OnSearchSelectHide()"
               alt=""/>
          <input type="text" id="MSearchField" value="Search" accesskey="S"
               onfocus="searchBox.OnSearchFieldFocus(true)" 
               onblur="searchBox.OnSearchFieldFocus(false)" 
               onkeyup="searchBox.OnSearchFieldChange(event)"/>
          </span><span class="right">
            <a id="MSearchClose" href="javascript:searchBox.CloseResultsWindow()"><img id="MSearchCloseImg" border="0" src="search/close.png" alt=""/></a>
          </span>
        </div>
      </li>
    </ul>
  </div>
</div>
<div class="header">
  <div class="summary">
<a href="#func-members">Functions</a>  </div>
  <div class="headertitle">
<div class="title">ZUPT aided INS functions</div>  </div>
<div class="ingroups"><a class="el" href="group__nav__eq.html">OpenShoe filtering algorithms</a></div></div>
<div class="contents">

<p>the main functions for running the ZUPT aided INS.  
<a href="#details">More...</a></p>
<table class="memberdecls">
<tr><td colspan="2"><h2><a name="func-members"></a>
Functions</h2></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__main__func.html#gaae1bbd924940610e1709300d40ca5a0f">update_imu_data_buffers</a> (void)</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Function that updates the IMU data buffers with the latest values read from the IMU, and writes the IMU data to that should be process at the current iteration to the processing variables.  <a href="#gaae1bbd924940610e1709300d40ca5a0f"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__main__func.html#ga9169808fbcb9f517deb908960a69ca7a">strapdown_mechanisation_equations</a> (void)</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Function for doing a time update of the mechanized navigation equations.  <a href="#ga9169808fbcb9f517deb908960a69ca7a"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__main__func.html#gac80ad2aa648edb47edb8f62883cb2a7d">time_up_data</a> (void)</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Function for doing a time update of the Kalman filter state covariance.  <a href="#gac80ad2aa648edb47edb8f62883cb2a7d"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__main__func.html#ga75c4a936b2db01f930ba89babb822f69">gain_matrix</a> (void)</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Function for calculating the Kalman filter gain matrix.  <a href="#ga75c4a936b2db01f930ba89babb822f69"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__main__func.html#gaa1673f6cf928e97c6372c56eddb4cc72">measurement_update</a> (void)</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Function for doing a measurement update of the Kalman filter covariance.  <a href="#gaa1673f6cf928e97c6372c56eddb4cc72"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__main__func.html#ga8812381ada5c2786a10e378e4ca34377">correct_navigation_states</a> (void)</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Function for correcting the navigation states given a zero-velocity detection.  <a href="#ga8812381ada5c2786a10e378e4ca34377"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__main__func.html#gaf0b7424f50d37755b6b0736808082299">ZUPT_detector</a> (void)</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Function for detecting when the system has zero-velocity.  <a href="#gaf0b7424f50d37755b6b0736808082299"></a><br/></td></tr>
</table>
<hr/><a name="details" id="details"></a><h2>Detailed Description</h2>
<p>the main functions for running the ZUPT aided INS. </p>
<hr/><h2>Function Documentation</h2>
<a class="anchor" id="ga8812381ada5c2786a10e378e4ca34377"></a><!-- doxytag: member="nav_eq.c::correct_navigation_states" ref="ga8812381ada5c2786a10e378e4ca34377" args="(void)" -->
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">void correct_navigation_states </td>
          <td>(</td>
          <td class="paramtype">void&#160;</td>
          <td class="paramname"></td><td>)</td>
          <td></td>
        </tr>
      </table>
</div>
<div class="memdoc">

<p>Function for correcting the navigation states given a zero-velocity detection. </p>
<p>When called this function takes the velocity estimate of the navigation system and treat it as an observation of the current velocity error of the system. The errors in all navigation states (position, velocity, and attitude) are then estimated by multiplying the "velocity error" with the Kalman gain. The estimated errors in the navigation states are then used to correct the navigation states.</p>
<dl><dt><b>Parameters:</b></dt><dd>
  <table class="params">
    <tr><td class="paramdir">[in,out]</td><td class="paramname">position</td><td>The position estimate of the navigation system. </td></tr>
    <tr><td class="paramdir">[in,out]</td><td class="paramname">velocity</td><td>The velocity estimate of the navigation system. </td></tr>
    <tr><td class="paramdir">[out]</td><td class="paramname">quaternions</td><td>The orientation estimate of the navigation system. </td></tr>
    <tr><td class="paramdir">[in]</td><td class="paramname">Rb2t</td><td>The vector representation of the body to navigation coordinate system rotation matrix estimate. </td></tr>
    <tr><td class="paramdir">[in]</td><td class="paramname">kalman_gain</td><td>The vector representation of the Kalman filter gain matrix. </td></tr>
  </table>
  </dd>
</dl>

</div>
</div>
<a class="anchor" id="ga75c4a936b2db01f930ba89babb822f69"></a><!-- doxytag: member="nav_eq.c::gain_matrix" ref="ga75c4a936b2db01f930ba89babb822f69" args="(void)" -->
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">void gain_matrix </td>
          <td>(</td>
          <td class="paramtype">void&#160;</td>
          <td class="paramname"></td><td>)</td>
          <td></td>
        </tr>
      </table>
</div>
<div class="memdoc">

<p>Function for calculating the Kalman filter gain matrix. </p>
<p>When called the function calculates the Kalman filter gain and store it in the vector <em>kalman_gain</em>. To calculate the gain the function uses the Kalman filter covariance and the pseudo velocity measurement noise standard deviation.</p>
<dl><dt><b>Parameters:</b></dt><dd>
  <table class="params">
    <tr><td class="paramdir">[out]</td><td class="paramname">kalman_gain</td><td>The vector representation of the Kalman filter gain matrix. </td></tr>
    <tr><td class="paramdir">[in]</td><td class="paramname">cov_vector</td><td>The vector representation of the Kalman filter covariance matrix. </td></tr>
    <tr><td class="paramdir">[in]</td><td class="paramname">sigma_velocity</td><td>The standard deviation of the pseudo velocity measurement error (Note: This parameters enters the function through a sub-function.). </td></tr>
  </table>
  </dd>
</dl>

</div>
</div>
<a class="anchor" id="gaa1673f6cf928e97c6372c56eddb4cc72"></a><!-- doxytag: member="nav_eq.c::measurement_update" ref="gaa1673f6cf928e97c6372c56eddb4cc72" args="(void)" -->
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">void measurement_update </td>
          <td>(</td>
          <td class="paramtype">void&#160;</td>
          <td class="paramname"></td><td>)</td>
          <td></td>
        </tr>
      </table>
</div>
<div class="memdoc">

<p>Function for doing a measurement update of the Kalman filter covariance. </p>
<p>When called the function does a measurement update of the Kalman filter state covariance matrix stored in the vector <em>cov_vector</em>.</p>
<dl><dt><b>Parameters:</b></dt><dd>
  <table class="params">
    <tr><td class="paramdir">[in,out]</td><td class="paramname">cov_vector</td><td>The vector representation of the Kalman filter covariance matrix. </td></tr>
    <tr><td class="paramdir">[in]</td><td class="paramname">kalman_gain</td><td>The vector representation of the Kalman filter gain matrix. </td></tr>
  </table>
  </dd>
</dl>

</div>
</div>
<a class="anchor" id="ga9169808fbcb9f517deb908960a69ca7a"></a><!-- doxytag: member="nav_eq.c::strapdown_mechanisation_equations" ref="ga9169808fbcb9f517deb908960a69ca7a" args="(void)" -->
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">void strapdown_mechanisation_equations </td>
          <td>(</td>
          <td class="paramtype">void&#160;</td>
          <td class="paramname"></td><td>)</td>
          <td></td>
        </tr>
      </table>
</div>
<div class="memdoc">

<p>Function for doing a time update of the mechanized navigation equations. </p>
<p>When called the function does a time update of the mechanized inertial navigation system equations. That is, first the quaternions stored in <em>quaternions</em> is updated using the angular rate measurements in <em>angular_rates_out</em>. Then the position and velocity state vectors <em>position</em> and <em>velocity</em> are updated using the acceleration measurements in <em>accelerations_out</em>. The function also updates the ''aiding'' vector <em>Rb2t</em> (body to navigation coordinate system rotation matrix) which is used in the Kalman filter.</p>
<dl class="note"><dt><b>Note:</b></dt><dd>This is a rudimentary mechanization of the inertial navigation equations which only is suitable for use with systems that uses low-cost/low-performance inertial sensors and where only short periods of free inertial navigation is expected.</dd></dl>
<dl><dt><b>Parameters:</b></dt><dd>
  <table class="params">
    <tr><td class="paramdir">[in,out]</td><td class="paramname">position</td><td>The position estimate of the navigation system. </td></tr>
    <tr><td class="paramdir">[in,out]</td><td class="paramname">velocity</td><td>The velocity estimate of the navigation system. </td></tr>
    <tr><td class="paramdir">[in,out]</td><td class="paramname">quaternions</td><td>The orientation estimate of the navigation system. </td></tr>
    <tr><td class="paramdir">[out]</td><td class="paramname">Rb2t</td><td>The body to navigation coordinate system rotation matrix estimate. </td></tr>
    <tr><td class="paramdir">[in]</td><td class="paramname">accelerations_out</td><td>The acceleration measurements used in the update of the inertial navigation system equations. </td></tr>
    <tr><td class="paramdir">[in]</td><td class="paramname">angular_rates_out</td><td>The angular rate measurements used in the update of the inertial navigation system equations. </td></tr>
    <tr><td class="paramdir">[in]</td><td class="paramname">dt</td><td>The sampling period of the system. </td></tr>
    <tr><td class="paramdir">[in]</td><td class="paramname">g</td><td>The magnitude of the local gravity vector. </td></tr>
  </table>
  </dd>
</dl>

</div>
</div>
<a class="anchor" id="gac80ad2aa648edb47edb8f62883cb2a7d"></a><!-- doxytag: member="nav_eq.c::time_up_data" ref="gac80ad2aa648edb47edb8f62883cb2a7d" args="(void)" -->
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">void time_up_data </td>
          <td>(</td>
          <td class="paramtype">void&#160;</td>
          <td class="paramname"></td><td>)</td>
          <td></td>
        </tr>
      </table>
</div>
<div class="memdoc">

<p>Function for doing a time update of the Kalman filter state covariance. </p>
<p>When called the function does a time update of the Kalman filter state covariance matrix stored in the vector <em>cov_vector</em>.</p>
<dl><dt><b>Parameters:</b></dt><dd>
  <table class="params">
    <tr><td class="paramdir">[in,out]</td><td class="paramname">cov_vector</td><td>The vector representation of the Kalman filter covariance matrix. </td></tr>
    <tr><td class="paramdir">[in]</td><td class="paramname">dt</td><td>The sampling period of the system. </td></tr>
    <tr><td class="paramdir">[in]</td><td class="paramname">sigma_acceleration</td><td>The standard deviation of the accelerometer process noise. </td></tr>
    <tr><td class="paramdir">[in]</td><td class="paramname">sigma_gyroscope</td><td>The standard deviation of the gyroscope process noise. </td></tr>
    <tr><td class="paramdir">[in]</td><td class="paramname">accelerations_out</td><td>The acceleration measurements used in the update of the inertial navigation system equations. </td></tr>
    <tr><td class="paramdir">[in]</td><td class="paramname">Rb2t</td><td>The vector current body to navigation coordinate system rotation matrix estimate. </td></tr>
  </table>
  </dd>
</dl>

</div>
</div>
<a class="anchor" id="gaae1bbd924940610e1709300d40ca5a0f"></a><!-- doxytag: member="nav_eq.c::update_imu_data_buffers" ref="gaae1bbd924940610e1709300d40ca5a0f" args="(void)" -->
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">void update_imu_data_buffers </td>
          <td>(</td>
          <td class="paramtype">void&#160;</td>
          <td class="paramname"></td><td>)</td>
          <td></td>
        </tr>
      </table>
</div>
<div class="memdoc">

<p>Function that updates the IMU data buffers with the latest values read from the IMU, and writes the IMU data to that should be process at the current iteration to the processing variables. </p>
<p>The function updates the IMU data buffers <em>acc_buffer_x_axis</em>, <em>acc_buffer_y_axis</em>, <em>acc_buffer_z_axis</em>, <em>gyro_buffer_x_axis</em>, <em>gyro_buffer_y_axis</em>, <em>gyro_buffer_z_axis</em> with the values stored in the vectors <em>accelerations_in</em> and <em>angular_rates_in</em>. The function also updates the vectors <em>accelerations_out</em> and <em>angular_rates_out</em>. The data stored in these vectors is the data processed in the next iteration of the navigation algorithm.</p>
<dl class="note"><dt><b>Note:</b></dt><dd>This function should be called after data have been read from the IMU through the SPI interface and before the navigation algorithm is processed.</dd></dl>
<dl><dt><b>Parameters:</b></dt><dd>
  <table class="params">
    <tr><td class="paramdir">[out]</td><td class="paramname">accelerations_out</td><td>The acceleration data that is to be used in the next iteration of the navigation algorithm. </td></tr>
    <tr><td class="paramdir">[out]</td><td class="paramname">angular_rates_out</td><td>The angular rate data that is to be used in the next iteration of the navigation algorithm. </td></tr>
    <tr><td class="paramdir">[in]</td><td class="paramname">accelerations_in</td><td>The from the IMU latest read acceleration data. </td></tr>
    <tr><td class="paramdir">[in]</td><td class="paramname">angular_rates_in</td><td>The from the IMU latest read angular rate data. </td></tr>
  </table>
  </dd>
</dl>

</div>
</div>
<a class="anchor" id="gaf0b7424f50d37755b6b0736808082299"></a><!-- doxytag: member="nav_eq.c::ZUPT_detector" ref="gaf0b7424f50d37755b6b0736808082299" args="(void)" -->
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">void ZUPT_detector </td>
          <td>(</td>
          <td class="paramtype">void&#160;</td>
          <td class="paramname"></td><td>)</td>
          <td></td>
        </tr>
      </table>
</div>
<div class="memdoc">

<p>Function for detecting when the system has zero-velocity. </p>
<p>When called this function takes the acceleration and angular rate measurements stored in the IMU data buffers and runs a generalized likelihood ratio test to determine if the navigation system is stationary or moving. More information about the detector and it characteristics can be found in following papers:</p>
<ul>
<li><a href="https://eeweb01.ee.kth.se/upload/publications/reports/2010/IR-EE-SB_2010_038.pdf">Zero-Velocity Detection -- An Algorithm Evaluation</a> </li>
<li><a href="https://eeweb01.ee.kth.se/upload/publications/reports/2010/IR-EE-SB_2010_043.pdf">Evaluation of Zero-Velocity Detectors for Foot-Mounted Inertial Navigation Systems</a></li>
</ul>
<dl><dt><b>Parameters:</b></dt><dd>
  <table class="params">
    <tr><td class="paramdir">[out]</td><td class="paramname">zupt</td><td>The zero-velocity detection flag </td></tr>
    <tr><td class="paramdir">[in]</td><td class="paramname">detector_Window_size</td><td>The window size of the zero-velocity detector. </td></tr>
    <tr><td class="paramdir">[in]</td><td class="paramname">detector_threshold</td><td>The threshold used in the detector. </td></tr>
    <tr><td class="paramdir">[in]</td><td class="paramname">sigma_acc_det</td><td>The standard deviation figure used to control the importance of the accelerometer measurements in the detection algorithm. </td></tr>
    <tr><td class="paramdir">[in]</td><td class="paramname">sigma_gyro_det</td><td>The standard deviation figure used to control the importance of the gyroscope measurements in the detection algorithm. </td></tr>
    <tr><td class="paramdir">[in]</td><td class="paramname">g</td><td>The magnitude of the local gravity vector. </td></tr>
  </table>
  </dd>
</dl>

</div>
</div>
</div>
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
<a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(0)"><span class="SelectionMark">&#160;</span>All</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(1)"><span class="SelectionMark">&#160;</span>Data Structures</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(2)"><span class="SelectionMark">&#160;</span>Files</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(3)"><span class="SelectionMark">&#160;</span>Functions</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(4)"><span class="SelectionMark">&#160;</span>Variables</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(5)"><span class="SelectionMark">&#160;</span>Typedefs</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(6)"><span class="SelectionMark">&#160;</span>Defines</a></div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>



<hr class="footer"/><address class="footer"><small>
Generated on Tue Dec 20 2011 12:12:37 for OpenShoe by &#160;<a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/>
</a> 1.7.5.1
</small></address>

</body>
</html>
